//#########################################################################
//                              WackiEngie
//           Copyright (c) 2014 Marc Wacker. All rights reserved.
//#########################################################################

#pragma once

#include "WeUtilPrerequisites.h"

#include "WeMath.h"
#include "WeVector2.h"
#include "WeVector3.h"
#include "WeVector4.h"
#include "WeMatrix2.h"
#include "WeMatrix3.h"

namespace WackiEngine
{

	template <class T>
	class Matrix4 {
	protected:
		T	_m[4][4];

	public:
		Matrix4();
		Matrix4(const T m0, const T m1, const T m2, const T m3,
			const T m4, const T m5, const T m6, const T m7,
			const T m8, const T m9, const T m10, const T m11,
			const T m12, const T m13, const T m14, const T m15);
		Matrix4(const Matrix2<T>& A);
		Matrix4(const Matrix3<T>& A);
		Matrix4(const Matrix4& A);

		Matrix4(const T tx, const T ty, const T tz);	//translation constructor
		Matrix4(const T degAng, const T rx, const T ry, const T rz);	//rotation constructor

		//setters
		void			setMatrix(const T m0 = 1, const T m1 = 0, const T m2 = 0, const T m3 = 0,
			const T m4 = 0, const T m5 = 1, const T m6 = 0, const T m7 = 0,
			const T m8 = 0, const T m9 = 0, const T m10 = 1, const T m11 = 0,
			const T m12 = 0, const T m13 = 0, const T m14 = 0, const T m15 = 1);

		void			setMatrix(const Matrix2<T>& A);
		void			setMatrix(const Matrix3<T>& A);
		void			setMatrix(const Matrix4& A);

		//getters
		T*				m() { return (T*)_m; }
		T				m(uint32 row, uint32 col)	{ return _m[row][col]; }

		void			multiply(const Matrix4 &A);

		//operators	
		Matrix4<T>&	operator=	(const Matrix4 &A);			//assignment
		Matrix4<T>	operator*	(const Matrix4 &A) const;		//matrix * matrix
		Matrix4<T>&	operator*=	(const Matrix4 &A);			//matrix *= matrix
		Vector3<T>	operator*	(const Vector3<T> &v) const;	//matrix * vector3
		Vector4<T>	operator*	(const Vector4<T> &v) const;	//matrix * vector4
		Matrix4<T>	operator*	(const T s) const;				//matrix * scalar
		Matrix4<T>&	operator*=	(const T s);					//matrix *= scalar
		Matrix4<T>	operator/	(const T s) const;				//matrix / scalar
		Matrix4<T>&	operator/=	(const T s);					//matrix /= scalar
		Matrix4<T>	operator+	(const Matrix4 &A) const;		//matrix + matrix
		Matrix4<T>&	operator+=	(const Matrix4 &A);			//matrix += matrix
		Matrix4<T>	operator-	(const Matrix4 &A) const;		//matrix - matrix
		Matrix4<T>&	operator-=	(const Matrix4 &A);			//matrix -= matrix

		//transformations
		void			translate(const T x, const T y = 0, const T z = 0);
		void			translate(const Vector2<T>& t);
		void			translate(const Vector3<T>& t);

		void			scale(const T sxyz);
		void			scale(const T sx, const T sy, const T sz);
		void			scale(const Vector3<T> &sxyz);

		void			rotate(const T degAng, const T x, const T y, const T z);
		void			rotate(const T degAng, const Vector3<T>& axis);
		void			rotation(const T degAng, const T x, const T y, const T z);	//set rotation

		void			translation(const T tx, const T ty, const T tz);	//sets the translation part of this matrix
		void			translation(const Vector3<T>& t);
		Vector3<T>		translation() const;								//returns the translation vector of this matrix


		void			frustum(const T left, const T right, const T bottom, const T top, const T near, const T far);

		void			perspective(const T fov, const T ar, const T near, const T far);

		void			ortho(const T left, const T right, const T bottom, const T top, const T near, const T far);
		void			ortho2D(const T width, const T height);

		void			lookAt(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up);
		void			lookAt(const Vector3<T> forward, const Vector3<T>& up);
		void			lookAt(Vector3<T>* eye, Vector3<T>* at, Vector3<T>* up, Vector3<T>* right) const;

		void			identity();
		Matrix4<T>		transposed() const;
		void			transpose();
		Matrix4<T>		inverse() const;
		void			invert();

	};


	//constructors
	template<class T>
	Matrix4<T>::Matrix4()
	{
		identity();
	}


	template<class T>
	Matrix4<T>::Matrix4(T m0, T m1, T m2, T m3,
		T m4, T m5, T m6, T m7,
		T m8, T m9, T m10, T m11,
		T m12, T m13, T m14, T m15)
	{
		setMatrix(m0, m1, m2, m3,
			m4, m5, m6, m7,
			m8, m9, m10, m11,
			m12, m13, m14, m15);
	}

	template<class T>
	Matrix4<T>::Matrix4(const Matrix2<T>& A)
	{
		setMatrix(A);
	}
	template<class T>
	Matrix4<T>::Matrix4(const Matrix3<T>& A)
	{
		setMatrix(A);
	}
	template<class T>
	Matrix4<T>::Matrix4(const Matrix4& A)
	{
		setMatrix(A);
	}

	template<class T>
	Matrix4<T>::Matrix4(const T tx, const T ty, const T tz)
	{
		identity();
		translation(tx, ty, tz);
	}

	template<class T>
	Matrix4<T>::Matrix4(const T degAng, const T rx, const T ry, const T rz)
	{
		identity();
		rotation(degAng, rx, ry, rz);
	}

	//setters
	template<class T>
	void Matrix4<T>::setMatrix(T m0, T m1, T m2, T m3,
		T m4, T m5, T m6, T m7,
		T m8, T m9, T m10, T m11,
		T m12, T m13, T m14, T m15)
	{
		_m[0][0] = m0;	_m[0][1] = m1;	_m[0][2] = m2;	_m[0][3] = m3;
		_m[1][0] = m4;	_m[1][1] = m5;	_m[1][2] = m6;	_m[1][3] = m7;
		_m[2][0] = m8;	_m[2][1] = m9;	_m[2][2] = m10;	_m[2][3] = m11;
		_m[3][0] = m12;	_m[3][1] = m13;	_m[3][2] = m14;	_m[3][3] = m15;
	}

	template<class T>
	void Matrix4<T>::setMatrix(const Matrix2<T>& A)
	{
		_m[0][0] = A._m[0][0];	_m[0][1] = A._m[0][1];	_m[0][2] = 0;	_m[0][3] = 0;
		_m[1][0] = A._m[1][0];	_m[1][1] = A._m[1][1];	_m[1][2] = 0;	_m[1][3] = 0;
		_m[2][0] = 0;			_m[2][1] = 0;			_m[2][2] = 1;	_m[2][3] = 0;
		_m[3][0] = 0;			_m[3][1] = 0;			_m[3][2] = 0;	_m[3][3] = 1;
	}

	template<class T>
	void Matrix4<T>::setMatrix(const Matrix3<T>& A)
	{
		_m[0][0] = A._m[0][0];	_m[0][1] = A._m[0][1];	_m[0][2] = A._m[0][2];	_m[0][3] = 0;
		_m[1][0] = A._m[1][0];	_m[1][1] = A._m[1][1];	_m[1][2] = A._m[1][2];	_m[1][3] = 0;
		_m[2][0] = A._m[2][0];	_m[2][1] = A._m[2][1];	_m[2][2] = A._m[2][2];	_m[2][3] = 0;
		_m[3][0] = 0;			_m[3][1] = 0;			_m[3][2] = 0;			_m[3][3] = 1;
	}

	template<class T>
	void Matrix4<T>::setMatrix(const Matrix4& A)
	{
		_m[0][0] = A._m[0][0];	_m[0][1] = A._m[0][1];	_m[0][2] = A._m[0][2];	_m[0][3] = A._m[0][3];
		_m[1][0] = A._m[1][0];	_m[1][1] = A._m[1][1];	_m[1][2] = A._m[1][2];	_m[1][3] = A._m[1][3];
		_m[2][0] = A._m[2][0];	_m[2][1] = A._m[2][1];	_m[2][2] = A._m[2][2];	_m[2][3] = A._m[2][3];
		_m[3][0] = A._m[3][0];	_m[3][1] = A._m[3][1];	_m[3][2] = A._m[3][2];	_m[3][3] = A._m[3][3];
	}

	template<class T>
	void Matrix4<T>::multiply(const Matrix4& A)
	{
		setMatrix(_m[0][0] * A._m[0][0] + _m[0][1] * A._m[1][0] + _m[0][2] * A._m[2][0] + _m[0][3] * A._m[3][0],	//row 1
			_m[0][0] * A._m[0][1] + _m[0][1] * A._m[1][1] + _m[0][2] * A._m[2][1] + _m[0][3] * A._m[3][1],
			_m[0][0] * A._m[0][2] + _m[0][1] * A._m[1][2] + _m[0][2] * A._m[2][2] + _m[0][3] * A._m[3][2],
			_m[0][0] * A._m[0][3] + _m[0][1] * A._m[1][3] + _m[0][2] * A._m[2][3] + _m[0][3] * A._m[3][3],

			_m[1][0] * A._m[0][0] + _m[1][1] * A._m[1][0] + _m[1][2] * A._m[2][0] + _m[1][3] * A._m[3][0],	//row 2
			_m[1][0] * A._m[0][1] + _m[1][1] * A._m[1][1] + _m[1][2] * A._m[2][1] + _m[1][3] * A._m[3][1],
			_m[1][0] * A._m[0][2] + _m[1][1] * A._m[1][2] + _m[1][2] * A._m[2][2] + _m[1][3] * A._m[3][2],
			_m[1][0] * A._m[0][3] + _m[1][1] * A._m[1][3] + _m[1][2] * A._m[2][3] + _m[1][3] * A._m[3][3],

			_m[2][0] * A._m[0][0] + _m[2][1] * A._m[1][0] + _m[2][2] * A._m[2][0] + _m[2][3] * A._m[3][0],	//row 3
			_m[2][0] * A._m[0][1] + _m[2][1] * A._m[1][1] + _m[2][2] * A._m[2][1] + _m[2][3] * A._m[3][1],
			_m[2][0] * A._m[0][2] + _m[2][1] * A._m[1][2] + _m[2][2] * A._m[2][2] + _m[2][3] * A._m[3][2],
			_m[2][0] * A._m[0][3] + _m[2][1] * A._m[1][3] + _m[2][2] * A._m[2][3] + _m[2][3] * A._m[3][3],

			_m[3][0] * A._m[0][0] + _m[3][1] * A._m[1][0] + _m[3][2] * A._m[2][0] + _m[3][3] * A._m[3][0],	//row 4
			_m[3][0] * A._m[0][1] + _m[3][1] * A._m[1][1] + _m[3][2] * A._m[2][1] + _m[3][3] * A._m[3][1],
			_m[3][0] * A._m[0][2] + _m[3][1] * A._m[1][2] + _m[3][2] * A._m[2][2] + _m[3][3] * A._m[3][2],
			_m[3][0] * A._m[0][3] + _m[3][1] * A._m[1][3] + _m[3][2] * A._m[2][3] + _m[3][3] * A._m[3][3]);
	}

	//operators	
	template<class T>
	Matrix4<T>& Matrix4<T>::operator=(const Matrix4& A)
	{
		_m[0][0] = A._m[0][0];	_m[0][1] = A._m[0][1];	_m[0][2] = A._m[0][2];	_m[0][3] = A._m[0][3];
		_m[1][0] = A._m[1][0];	_m[1][1] = A._m[1][1];	_m[1][2] = A._m[1][2];	_m[1][3] = A._m[1][3];
		_m[2][0] = A._m[2][0];	_m[2][1] = A._m[2][1];	_m[2][2] = A._m[2][2];	_m[2][3] = A._m[2][3];
		_m[3][0] = A._m[3][0];	_m[3][1] = A._m[3][1];	_m[3][2] = A._m[3][2];	_m[3][3] = A._m[3][3];

		return *this;
	}

	template<class T>
	Matrix4<T> Matrix4<T>::operator*(const Matrix4& A) const
	{
		Matrix4<T> result((*this));
		result.multiply(A);
		return result;
	}

	template<class T>
	Matrix4<T>& Matrix4<T>::operator*=(const Matrix4& A)
	{
		multiply(A);
		return *this;
	}

	//Matrix - 3D vector multiplication with perspective division
	template<class T>
	Vector3<T> Matrix4<T>::operator*(const Vector3<T>& v) const
	{
		T W = _m[3][0] * v.x + _m[3][1] * v.y + _m[3][2] * v.z + _m[3][3];
		return Vector3<T>((_m[0][0] * v.x + _m[0][1] * v.y + _m[0][2] * v.z + _m[0][3]) / W,
			(_m[1][0] * v.x + _m[1][1] * v.y + _m[1][2] * v.z + _m[1][3]) / W,
			(_m[2][0] * v.x + _m[2][1] * v.y + _m[2][2] * v.z + _m[2][3]) / W);
	}

	template<class T>
	Vector4<T> Matrix4<T>::operator*(const Vector4<T>& v) const
	{
		return Vector4<T>(_m[0][0] * v.x + _m[0][1] * v.y + _m[0][2] * v.z + _m[0][3] * v.w,
			_m[1][0] * v.x + _m[1][1] * v.y + _m[1][2] * v.z + _m[1][3] * v.w,
			_m[2][0] * v.x + _m[2][1] * v.y + _m[2][2] * v.z + _m[2][3] * v.w,
			_m[3][0] * v.x + _m[3][1] * v.y + _m[3][2] * v.z + _m[3][3] * v.w);
	}

	template<class T>
	Matrix4<T> Matrix4<T>::operator*(const T s) const
	{
		return Matrix4<T>(_m[0][0] * s, _m[0][1] * s, _m[0][2] * s, _m[0][3] * s,
			_m[1][0] * s, _m[1][1] * s, _m[1][2] * s, _m[1][3] * s,
			_m[2][0] * s, _m[2][1] * s, _m[2][2] * s, _m[2][3] * s,
			_m[3][0] * s, _m[3][1] * s, _m[3][2] * s, _m[3][3] * s);
	}

	template<class T>
	Matrix4<T>& Matrix4<T>::operator*=(const T s)
	{
		for(int col = 0; col < 4; ++col)
		for(int row = 0; row < 4; ++row)
			_m[row][col] *= s;

		return *this;
	}

	template<class T>
	Matrix4<T> Matrix4<T>::operator/(const T s) const
	{
		return Matrix4<T>(_m[0][0] / s, _m[0][1] / s, _m[0][2] / s, _m[0][3] / s,
			_m[1][0] / s, _m[1][1] / s, _m[1][2] / s, _m[1][3] / s,
			_m[2][0] / s, _m[2][1] / s, _m[2][2] / s, _m[2][3] / s,
			_m[3][0] / s, _m[3][1] / s, _m[3][2] / s, _m[3][3] / s);
	}

	template<class T>
	Matrix4<T>& Matrix4<T>::operator/=(const T s)
	{
		for(int col = 0; col < 4; ++col)
		for(int row = 0; row < 4; ++row)
			_m[row][col] /= s;

		return *this;
	}

	template<class T>
	Matrix4<T> Matrix4<T>::operator+(const Matrix4& A) const
	{
		return Matrix4<T>(_m[0][0] + A._m[0][0], _m[0][1] + A._m[0][1], _m[0][2] + A._m[0][2], _m[0][3] + A._m[0][3],
			_m[1][0] + A._m[1][0], _m[1][1] + A._m[1][1], _m[1][2] + A._m[1][2], _m[1][3] + A._m[1][3],
			_m[2][0] + A._m[2][0], _m[2][1] + A._m[2][1], _m[2][2] + A._m[2][2], _m[2][3] + A._m[2][3],
			_m[3][0] + A._m[3][0], _m[3][1] + A._m[3][1], _m[3][2] + A._m[3][2], _m[3][3] + A._m[3][3]);
	}

	template<class T>
	Matrix4<T>& Matrix4<T>::operator+=(const Matrix4& A)
	{
		for(int col = 0; col < 4; ++col)
		for(int row = 0; row < 4; ++row)
			_m[row][col] += A._m[row][col];

		return *this;
	}

	template<class T>
	Matrix4<T> Matrix4<T>::operator-(const Matrix4& A) const
	{
		return Matrix4<T>(_m[0][0] - A._m[0][0], _m[0][1] - A._m[0][1], _m[0][2] - A._m[0][2], _m[0][3] - A._m[0][3],
			_m[1][0] - A._m[1][0], _m[1][1] - A._m[1][1], _m[1][2] - A._m[1][2], _m[1][3] - A._m[1][3],
			_m[2][0] - A._m[2][0], _m[2][1] - A._m[2][1], _m[2][2] - A._m[2][2], _m[2][3] - A._m[2][3],
			_m[3][0] - A._m[3][0], _m[3][1] - A._m[3][1], _m[3][2] - A._m[3][2], _m[3][3] - A._m[3][3]);
	}

	template<class T>
	Matrix4<T>& Matrix4<T>::operator-=(const Matrix4& A)
	{
		for(int col = 0; col < 4; ++col)
		for(int row = 0; row < 4; ++row)
			_m[row][col] -= A._m[row][col];

		return *this;
	}

	//transformations
	template<class T>
	void Matrix4<T>::translate(const T x, const T y = 0, const T z = 0)
	{
		Matrix4<T> trans(x, y, z);
		multiply(trans);
	}

	template<class T>
	void Matrix4<T>::translate(const Vector2<T>& t)
	{
		Matrix4<T> trans(t.x, t.y, 0);
		multiply(trans);
	}

	template<class T>
	void Matrix4<T>::translate(const Vector3<T>& t)
	{
		Matrix4<T> trans(t.x, t.y, t.z);
		multiply(trans);
	}

	template<class T>
	void Matrix4<T>::scale(const T s)
	{
		Matrix4<T> S(s, 0, 0, 0,
			0, s, 0, 0,
			0, 0, s, 0,
			0, 0, 0, 1);
		multiply(S);
	}

	template<class T>
	void Matrix4<T>::scale(const T sx, const T sy, const T sz)
	{
		Matrix4<T> S(sx, 0, 0, 0,
			0, sy, 0, 0,
			0, 0, sz, 0,
			0, 0, 0, 1);
		multiply(S);
	}

	template<class T>
	void Matrix4<T>::scale(const Vector3<T>& s)
	{
		Matrix4<T> S(s.x, 0, 0, 0,
			0, s.y, 0, 0,
			0, 0, s.z, 0,
			0, 0, 0, 1);
		multiply(S);
	}

	template<class T>
	void Matrix4<T>::rotate(const T degAng, const T x, const T y, const T z)
	{
		Matrix4<T> rot(degAng, x, y, z);
		multiply(rot);
	}

	template<class T>
	void Matrix4<T>::rotate(const T degAng, const Vector3<T> &axis)
	{
		Matrix4<T> rot(degAng, axis.x, axis.y, axis.z);
		multiply(rot);
	}

	template<class T>
	void Matrix4<T>::rotation(const T degAng, const T axisx, const T axisy, const T axisz)
	{
		T radAng = (T)degAng*WE_DEG2RAD;
		T ca = (T)cos(radAng), sa = (T)sin(radAng);
		if(axisx == 1 && axisy == 0 && axisz == 0)				// about x-axis
		{
			_m[0][0] = 1; _m[0][1] = 0;  _m[0][2] = 0;
			_m[1][0] = 0; _m[1][1] = ca; _m[1][2] = -sa;
			_m[2][0] = 0; _m[2][1] = sa; _m[2][2] = ca;
		}
		else if(axisx == 0 && axisy == 1 && axisz == 0)			// about y-axis
		{
			_m[0][0] = ca;  _m[0][1] = 0; _m[0][2] = sa;
			_m[1][0] = 0;   _m[1][1] = 1; _m[1][2] = 0;
			_m[2][0] = -sa; _m[2][1] = 0; _m[2][2] = ca;
		}
		else if(axisx == 0 && axisy == 0 && axisz == 1)			// about z-axis
		{
			_m[0][0] = ca; _m[0][1] = -sa; _m[0][2] = 0;
			_m[1][0] = sa; _m[1][1] = ca;  _m[1][2] = 0;
			_m[2][0] = 0;  _m[2][1] = 0;   _m[2][2] = 1;
		}
		else												// arbitrary axis
		{
			T l = axisx*axisx + axisy*axisy + axisz*axisz;  // length squared
			T x, y, z;
			x = axisx, y = axisy, z = axisz;
			if((l > T(1.0001) || l < T(0.9999)) && l != 0)
			{
				l = T(1.0) / sqrt(l);
				x *= l; y *= l; z *= l;
			}

			T xy = x*y, yz = y*z, xz = x*z, xx = x*x, yy = y*y, zz = z*z;
			_m[0][0] = xx + ca*(1 - xx);     _m[0][1] = xy - xy*ca - z*sa;  _m[0][2] = xz - xz*ca + y*sa;
			_m[1][0] = xy - xy*ca + z*sa;  _m[1][1] = yy + ca*(1 - yy);     _m[1][2] = yz - yz*ca - x*sa;
			_m[2][0] = xz - xz*ca - y*sa;  _m[2][1] = yz - yz*ca + x*sa;  _m[2][2] = zz + ca*(1 - zz);
		}

		_m[3][0] = _m[3][1] = _m[3][2] = 0; _m[3][3] = 1;
	}

	template<class T>
	void Matrix4<T>::translation(const T tx, const T ty, const T tz)
	{
		_m[0][3] = tx;
		_m[1][3] = ty;
		_m[2][3] = tz;
	}

	template<class T>
	void Matrix4<T>::translation(const Vector3<T> &t)
	{
		_m[0][3] = t.x;
		_m[1][3] = t.y;
		_m[2][3] = t.z;
	}

	template<class T>
	Vector3<T> Matrix4<T>::translation() const
	{
		return Vector3<T>(_m[0][3], _m[1][3], _m[2][3]);
	}

	template<class T>
	void Matrix4<T>::frustum(const T l, const T r, const T b, const T t, const T n, const T f)
	{
		_m[0][0] = (2 * n) / (r - l);	_m[0][1] = 0;			_m[0][2] = (r + l) / (r - l);		_m[0][3] = 0;
		_m[1][0] = 0;			_m[1][1] = (2 * n) / (t - b);	_m[1][2] = (t + b) / (t - b);		_m[1][3] = 0;
		_m[2][0] = 0;			_m[2][1] = 0;			_m[2][2] = -(f + n) / (f - n);	_m[2][3] = (-2 * f*n) / (f - n);
		_m[3][0] = 0;			_m[3][1] = 0;			_m[3][2] = -1;				_m[3][3] = 0;
	}

	template<class T>
	void Matrix4<T>::perspective(const T fov, const T ar, const T n, const T f)
	{
		T tanHalf = tanf(DEG2RAD*fov * 0.5f);

		_m[0][0] = 1 / (ar*tanHalf);	_m[0][1] = 0;			_m[0][2] = 0;				_m[0][3] = 0;
		_m[1][0] = 0;				_m[1][1] = 1 / tanHalf;	_m[1][2] = 0;				_m[1][3] = 0;
		_m[2][0] = 0;				_m[2][1] = 0;			_m[2][2] = -(f + n) / (f - n);	_m[2][3] = (-2 * f*n) / (f - n);
		_m[3][0] = 0;				_m[3][1] = 0;			_m[3][2] = -1;				_m[3][3] = 0;
	}

	template<class T>
	void Matrix4<T>::ortho(const T left, const T right, const T bottom, const T top, const T n, const T f)
	{
		_m[0][0] = 2 / (right - left);	_m[0][1] = 0;				_m[0][2] = 0;		_m[0][3] = -(right + left) / (right - left);
		_m[1][0] = 0;				_m[1][1] = 2 / (top - bottom);	_m[1][2] = 0;		_m[1][3] = -(top + bottom) / (top - bottom);
		_m[2][0] = 0;				_m[2][1] = 0;				_m[2][2] = 2 / (f - n);	_m[2][3] = -(f + n) / (f - n);
		_m[3][0] = 0;				_m[3][1] = 0;				_m[3][2] = 0;		_m[3][3] = 1;
	}

	template<class T>
	void Matrix4<T>::ortho2D(const T width, const T height)
	{
		ortho(0, width, 0, height, -1, 1);
	}

	template<class T>
	void Matrix4<T>::lookAt(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
	{
		Vector3<T> VX, VY, VZ, VT;
		Matrix4<T> xz(0.0, 0.0, 1.0, 0.0,         // matrix that transforms YZ into a 
			0.0, 0.0, 0.0, 0.0,         // vector that is perpendicular to YZ and 
			-1.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 1.0);        // lies in the x-z plane

		VZ = eye - at;
		VZ.normalize();

		if(up == Vector3<T>() || up == VZ) {
			VX = (xz*VZ);		VX.normalize();
		}
		else {
			VX = up.cross(VZ);	VX.normalize();
		}

		VY = VZ.cross(VX); VY.normalize();
		VT = -eye;

		setMatrix(VX.x, VX.y, VX.z, VX*VT,
			VY.x, VY.y, VY.z, VY*VT,
			VZ.x, VZ.y, VZ.z, VZ*VT,
			0.0, 0.0, 0.0, 1.0);
	}

	template<class T>
	void Matrix4<T>::lookAt(const Vector3<T> forward, const Vector3<T>& up)
	{
		Vector3<T> VX, VY, VZ, VT;
		Matrix4<T> xz(0.0, 0.0, 1.0, 0.0,         // matrix that transforms VZ into a 
			0.0, 0.0, 0.0, 0.0,					// vector that is perpendicular to VZ and 
			-1.0, 0.0, 0.0, 0.0,
			0.0, 0.0, 0.0, 1.0);				// lies in the x-z plane

		VZ = forward;
		VZ.normalize();

		if(up == Vector3<T>() || up == VZ) {
			VX = (xz*VZ);
			VX.normalize();
		}
		else {
			VX = up.cross(VZ);
			VX.normalize();
		}

		VY = VZ.cross(VX); VY.normalize();
		VT = Vector3<T>(0, 0, 0);

		setMatrix(VX.x, VX.y, VX.z, VX*VT,
			VY.x, VY.y, VY.z, VY*VT,
			VZ.x, VZ.y, VZ.z, VZ*VT,
			0.0, 0.0, 0.0, 1.0);
	}

	template<class T>
	void Matrix4<T>::lookAt(Vector3<T>* eye, Vector3<T>* at, Vector3<T>* up, Vector3<T>* right) const
	{
		Matrix4<T> invRot(*this);				// get the current view matrix
		invRot.translation(0, 0, 0);			// remove the translation
		invRot.transpose();					// transpose it to get inverse rot.
		*eye = (invRot*-translation());	// setMatrix eye
		right->set(_m[0][0], _m[0][1], _m[0][2]);		// normalized look right vector
		up->set(_m[1][0], _m[1][1], _m[1][2]);		// normalized look up vector
		at->set(-_m[2][0], -_m[2][1], -_m[2][2]);		// normalized look at vector
	}

	template<class T>
	void Matrix4<T>::identity()
	{
		_m[0][0] = _m[1][1] = _m[2][2] = _m[3][3] = 1;
		_m[0][1] = _m[0][2] = _m[0][3] = _m[1][0] = _m[1][2] = _m[1][3] = _m[2][0] = _m[2][1] = _m[2][3] = _m[3][0] = _m[3][1] = _m[3][2] = 0;
	}

	template<class T>
	Matrix4<T> Matrix4<T>::transposed() const
	{
		Matrix4<T> result((*this));
		result.transpose();
		return result;
	}

	template<class T>
	void Matrix4<T>::transpose()
	{
		std::swap(_m[0][1], _m[1][0]);
		std::swap(_m[0][2], _m[2][0]);
		std::swap(_m[0][3], _m[3][0]);
		std::swap(_m[1][2], _m[2][1]);
		std::swap(_m[1][3], _m[3][1]);
		std::swap(_m[2][3], _m[3][2]);
	}


	template<class T>
	void Matrix4<T>::invert()
	{
		setMatrix(inverse());
	}

	//-----------------------------------------------------------------------------
	//! Computes the inverse of a 4x4 non-singular matrix.
	template<class T>
	Matrix4<T> Matrix4<T>::inverse() const
	{
		Matrix4<T> i;

		// Code from Mesa-2.2\src\glu\project.c
		T det, d12, d13, d23, d24, d34, d41;

		// Inverse = adjoint / det. (See linear algebra texts.)
		// pre-compute 2x2 dets for last two rows when computing
		// cof_actors of first two rows.
		d12 = (_m[2][0] * _m[3][1] - _m[3][0] * _m[2][1]);
		d13 = (_m[2][0] * _m[3][2] - _m[3][0] * _m[2][2]);
		d23 = (_m[2][1] * _m[3][2] - _m[3][1] * _m[2][2]);
		d24 = (_m[2][1] * _m[3][3] - _m[3][1] * _m[2][3]);
		d34 = (_m[2][2] * _m[3][3] - _m[3][2] * _m[2][3]);
		d41 = (_m[2][3] * _m[3][0] - _m[3][3] * _m[2][0]);

		i._m[0][0] = (_m[1][1] * d34 - _m[1][2] * d24 + _m[1][3] * d23);
		i._m[1][0] = -(_m[1][0] * d34 + _m[1][2] * d41 + _m[1][3] * d13);
		i._m[2][0] = (_m[1][0] * d24 + _m[1][1] * d41 + _m[1][3] * d12);
		i._m[3][0] = -(_m[1][0] * d23 - _m[1][1] * d13 + _m[1][2] * d12);

		// Compute determinant as early as possible using these cof_actors.
		det = _m[0][0] * i._m[0][0] + _m[0][1] * i._m[1][0] + _m[0][2] * i._m[2][0] + _m[0][3] * i._m[3][0];

		// Run singularity test.
		if(std::fabs(det) <= 0.0001)
		{
			std::cout << "4x4-Matrix is singular. Inversion impossible." << std::endl;
			std::cout << std::endl;
			exit(0);
		}
		else
		{
			T invDet = 1 / det;
			// Compute rest of inverse.
			i._m[0][0] *= invDet;
			i._m[1][0] *= invDet;
			i._m[2][0] *= invDet;
			i._m[3][0] *= invDet;

			i._m[0][1] = -(_m[0][1] * d34 - _m[0][2] * d24 + _m[0][3] * d23)*invDet;
			i._m[1][1] = (_m[0][0] * d34 + _m[0][2] * d41 + _m[0][3] * d13)*invDet;
			i._m[2][1] = -(_m[0][0] * d24 + _m[0][1] * d41 + _m[0][3] * d12)*invDet;
			i._m[3][1] = (_m[0][0] * d23 - _m[0][1] * d13 + _m[0][2] * d12)*invDet;

			// Pre-compute 2x2 dets for first two rows when computing
			// cofactors of last two rows.
			d12 = _m[0][0] * _m[1][1] - _m[1][0] * _m[0][1];
			d13 = _m[0][0] * _m[1][2] - _m[1][0] * _m[0][2];
			d23 = _m[0][1] * _m[1][2] - _m[1][1] * _m[0][2];
			d24 = _m[0][1] * _m[1][3] - _m[1][1] * _m[0][3];
			d34 = _m[0][2] * _m[1][3] - _m[1][2] * _m[0][3];
			d41 = _m[0][3] * _m[1][0] - _m[1][3] * _m[0][0];

			i._m[0][2] = (_m[3][1] * d34 - _m[3][2] * d24 + _m[3][3] * d23)*invDet;
			i._m[1][2] = -(_m[3][0] * d34 + _m[3][2] * d41 + _m[3][3] * d13)*invDet;
			i._m[2][2] = (_m[3][0] * d24 + _m[3][1] * d41 + _m[3][3] * d12)*invDet;
			i._m[3][2] = -(_m[3][0] * d23 - _m[3][1] * d13 + _m[3][2] * d12)*invDet;
			i._m[0][3] = -(_m[2][1] * d34 - _m[2][2] * d24 + _m[2][3] * d23)*invDet;
			i._m[1][3] = (_m[2][0] * d34 + _m[2][2] * d41 + _m[2][3] * d13)*invDet;
			i._m[2][3] = -(_m[2][0] * d24 + _m[2][1] * d41 + _m[2][3] * d12)*invDet;
			i._m[3][3] = (_m[2][0] * d23 - _m[2][1] * d13 + _m[2][2] * d12)*invDet;
		}
		return i;
	}

	typedef Matrix4<float> Matrix4f;

}